/*
 * Controller.h
 *
 *  Created on: Apr 24, 2011
 *      Author: tflanzer
 */

#ifndef CONTROLLER_H_
#define CONTROLLER_H_

#include <cstdio>
#include <stdio.h>
#include <list>
#include <math.h>
#include <stdlib.h>
#include "timer.h"
#include "Servo.h"
#include "Communication.h"
#include "execStruct.h"
#include "EKF.h"
#include "Matrix.h"
#include "mavlink.h"
#include "FileWriter.h"
#include "Guidance.h"

class Controller: public Timer::fiftyHzListener, public Communication::GNDListener {
public:
	int faup, fgnd;

	Servo* aileron;
	Servo* elevator;
	Servo* rudder;
	Servo* throttle;

	double ail_PWM, ele_PWM, rud_PWM, thr_PWM, ail_lim, ele_lim, rud_lim, Vtarget,
		   phides, dDes, psiDes;
	int nums, numwps, sysctrLon, sysctrLat, ctr;
	bool flag, sysflagLon, sysflagLat, vertManFlg;

	// Controller constants
	double U0; // Reference velocity for determine bank angle
	double taup; // psi/psi_desired = 1/(taup*s+1) - smooths out noisy psi_dot
	double philim; // Maximum bank angle that can be commanded

	double xe[3], xeL[3], phiL, rL, reL, re[2], VaL, Va_err_i[2], V_L1, hL;

	// Error states
	double phiErr, rErr, dErr, vErr;

	FileWriter *fw;
	Guidance *gnc;

	Controller(int fa, int fg, FileWriter *fw, Guidance *gnc);
	virtual ~Controller();
	void fiftyHzUpdate(Timer &timer);
	void GNDdataReceived(Communication &com);
	double wpCommand();
	void sendServoCommands();
	void cross(double *a, double *b, double *c);
	void loadGains();
	void TECSlongitudinal();
	void sysIDLong();
	void sysIDLat();

private:
	double computeAirspeed();
	double computeBeta();
	void lateralPID(double phides);
	void pbeta(double phides);
	void airspeedPID();
	void LQRPI_V_alt();
	void body2inertial(double psi, double th, double phi, double *x);
	void inertial2body(double psi, double th, double phi, double *x);
	void updateControlFile();
};

#endif /* CONTROLLER_H_ */
